#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>

#include "KinematicModel\Point.h"
#include "KinematicModel\KinematicModelTypes.h"
#include <boost\thread.hpp>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
#include "OpenNIScreenCapturer.h"

void task(OpenNIScreenCapturer* o){

	o->run();

}



std::vector<Point> classifier(int movementClass, OpenNIScreenCapturer* o);
invKinResult invKin(Point p, bool recursive);

int main(int argc, char* argv[]){

	OpenNIScreenCapturer o; 
	boost::thread t = boost::thread(task, &o);

	std::cout << "Initializing Screen Capturer" << std::endl;
	//Sleep(10000);
	std::cout << "Screen Capturer Initialized" << std::endl;
	

	while(true){

		int movementClass;

		printf("Insert movement class: ");
		scanf("%d", &movementClass);

		invKinResult ikr;
		std::vector<Point> path = classifier(movementClass, &o);
	
	
		std::ofstream myfile;
		myfile.open ("C:\\Tesi\\matlab.txt", std::ios::trunc);
		Point p;
		myfile << "function [ out ] = Prova( in ) \n";
		int i;
		for(i = 1; i < path.size(); i++){

			p = Point(path.at(i-1).getValue(Point::X), path.at(i-1).getValue(Point::Y), path.at(i-1).getValue(Point::Z));
			ikr = invKin(p, true);

			myfile << "%Punto(" << path.at(i).getValue(Point::X) << ", " << path.at(i).getValue(Point::Y) << ", " <<  path.at(i).getValue(Point::Z) << ");" << std::endl;
			myfile << "moveFromTo(" << ikr.teta1 << ", " << ikr.teta2 << ", " <<  ikr.teta3 << ",";
		
			p = Point(path.at(i).getValue(Point::X), path.at(i).getValue(Point::Y), path.at(i).getValue(Point::Z));
			ikr = invKin(p, true);

			
			myfile << ikr.teta1 << ", " << ikr.teta2 << ", " <<  ikr.teta3 << ");" << std::endl << "pause(0.20);" << std::endl;
			//myfile << "cin_inv(" << path.at(i).getValue(Point::X) << ", " << path.at(i).getValue(Point::Y) << ", " <<  path.at(i).getValue(Point::Z) << ");" << std::endl << "pause(0.20);" << std::endl;

		}
	
		myfile << "\nend";

		myfile.close();
	}

	return 0;
}

